import rclpy
import cv2
import cv_bridge
import numpy as np
from rclpy.node import Node
from sensor_msgs.msg import CompressedImage
from origincar_msg.msg import Sign
from geometry_msgs.msg import Twist
import time

class Follower(Node):
    def __init__(self):
        super().__init__('line_follower')
        self.get_logger().info("Start line follower.")
        
        self.bridge = cv_bridge.CvBridge()
        
        self.image_sub = self.create_subscription(CompressedImage, '/image/compressed', self.image_callback, 10)
        self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
        self.qr_pub = self.create_publisher(Sign, '/sign_switch', 10)
        self.pub = self.create_publisher(CompressedImage, '/camera/process_image', 10)
    
        self.twist = Twist()
        self.qrDecoder = cv2.QRCodeDetector()  # Create a qrCodeDetector Object
    
    def image_callback(self, msg):
        self.get_logger().info("Image callback triggered.")
        try:
            np_arr = np.frombuffer(msg.data, np.uint8)
            image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        except cv_bridge.CvBridgeError as e:
            self.get_logger().error(f"CvBridge Error: {e}")
            return
        
        # Detect and decode the QR code
        data, bbox, rectifiedImage = self.qrDecoder.detectAndDecode(image)
        if data:
            self.get_logger().info(f"QR Code detected: {data}")
            qr_msg = Sign()  # 创建一个Sign消息对象
            self.twist.linear.x = 0.0
            self.twist.angular.z = 0.0
            self.cmd_vel_pub.publish(self.twist)
            if data == 'Clockwise':
            # 设置sign_data的值为3，表示二维码数据为顺时针
                sign_data = 3
            else:
                sign_data = 4
            self.get_logger().info(f'Publishing: {qr_msg.sign_data}')
            self.qr_pub.publish(qr_msg)  # 发布消息到/sign_switch话题
            return
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
        lower_black = np.array([0, 10, 10])
        upper_black = np.array([255, 50, 90])
        # kernel = np.ones((5,5), np.uint8)
        mask = cv2.inRange(hsv, lower_black, upper_black)

        h, w, d = image.shape
        search_top = int(h * 2 / 3)
        search_bot = int(h / 2 + 20)
        mask[0:search_top, 0:w] = 0
        M = cv2.moments(mask)
        
        if M['m00'] > 0:
            cx = int(M['m10'] / M['m00'])
            cy = int(M['m01'] / M['m00'])
            cv2.circle(image, (cx, cy), 20, (0, 0, 255), -1)
            
            # 基于检测的目标中心点，计算机器人的控制参数
            err = cx - w / 2
            self.twist.linear.x = 0.2   #巡线的速度
            self.twist.angular.z = -float(err) / 380 #巡线调节的角度P调整
            self.cmd_vel_pub.publish(self.twist)

        try:
            compressed_image = CompressedImage()
            compressed_image.header.stamp = self.get_clock().now().to_msg()
            compressed_image.format = "jpeg"
            compressed_image.data = np.array(cv2.imencode('.jpg', image)[1]).tobytes()
            self.pub.publish(compressed_image)
        except cv_bridge.CvBridgeError as e:
            self.get_logger().error(f"Publish CvBridge Error: {e}")

def main(args=None):
    rclpy.init(args=args)
    follower = Follower()
    rclpy.spin(follower)
    follower.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
